{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "62d6e8c5",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/robot.html).  I recommend having both windows open, side-by-side!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "71d3df05",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "eeMrMI0-1Dhu",
    "lines_to_end_of_cell_marker": 2
   },
   "outputs": [],
   "source": [
    "from pydrake.all import ModelVisualizer, Simulator, StartMeshcat\n",
    "\n",
    "from manipulation import ConfigureParser, running_as_notebook\n",
    "from manipulation.station import MakeHardwareStation, load_scenario"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "441ed96a",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2cdfa9c4",
   "metadata": {},
   "source": [
    "# IIWA with a mobile base\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5d154f2c",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "5SjOClhTltPk"
   },
   "outputs": [],
   "source": [
    "visualizer = ModelVisualizer(meshcat=meshcat)\n",
    "ConfigureParser(visualizer.parser())\n",
    "visualizer.AddModels(\n",
    "    url=\"package://manipulation/mobile_iiwa14_primitive_collision.urdf\"\n",
    ")\n",
    "visualizer.Run(loop_once=not running_as_notebook)\n",
    "meshcat.DeleteAddedControls()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ed3b9f5f",
   "metadata": {},
   "source": [
    "Here is a version using the HardwareStation interface.  Note that we use a generic InverseDynamics driver instead of the existing IiwaDriver (and the ports are now more generic, too), because we need the controller to also reason about the mobile base joints."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a78d2207",
   "metadata": {},
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_model:\n",
    "    name: mobile_iiwa\n",
    "    file: package://manipulation/mobile_iiwa14_primitive_collision.urdf\n",
    "    default_joint_positions:\n",
    "        iiwa_joint_1: [-1.57]\n",
    "        iiwa_joint_2: [0.1]\n",
    "        iiwa_joint_3: [0]\n",
    "        iiwa_joint_4: [-1.2]\n",
    "        iiwa_joint_5: [0]\n",
    "        iiwa_joint_6: [ 1.6]\n",
    "        iiwa_joint_7: [0]\n",
    "model_drivers:\n",
    "    mobile_iiwa: !InverseDynamicsDriver {}\n",
    "\"\"\"\n",
    "\n",
    "scenario = load_scenario(data=scenario_data)\n",
    "station = MakeHardwareStation(scenario, meshcat)\n",
    "simulator = Simulator(station)\n",
    "context = simulator.get_mutable_context()\n",
    "x0 = station.GetOutputPort(\"mobile_iiwa.state_estimated\").Eval(context)\n",
    "station.GetInputPort(\"mobile_iiwa.desired_state\").FixValue(context, x0)\n",
    "simulator.AdvanceTo(0.1)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.10.8 64-bit",
   "language": "python",
   "name": "python3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
